#include "../EmbStdLib/EmbStdLib_Type.h"
#include "../EmbStdLib/EmbStdLib_Malloc.h"
#include "../EmbStdLib/EmbStdLib_Queue.h"

uint32 main()
{
  // Init memory pool
  memory_init();

  // Create multiple queues
  Queue queue_instance1;
  Queue *queue1 = &queue_instance1;
  Queue queue_instance2;
  Queue *queue2 = &queue_instance2;

  // Initialize queues and specify global static arrays if dynamic memory allocation is disabled
  uint8 queue1_ret = QueueInit(TRUE, LOG_LEVEL_DEBUG, queue1, MAX_QUEUE1_MESSAGES, MAX_QUEUE1_MESSAGE_LENGTH, NULL_PTR);
  uint8 queue2_ret = QueueInit(TRUE, LOG_LEVEL_DEBUG, queue2, MAX_QUEUE2_MESSAGES, MAX_QUEUE2_MESSAGE_LENGTH, NULL_PTR);

  if (queue1_ret || queue2_ret)
  {
    printf("Failed to create queues\n");
    return QUEUE_ERROR_MEMORY;
  }

  // Messages for queue1
  const uint8 messages1_1[8] = {0x01, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x00};
  const uint8 messages1_2[8] = {0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x00};
  const uint8 messages1_3[8] = {0x01, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x00};
  const uint8 messages1_4[8] = {0x01, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x00};

  // Messages for queue2
  const uint8 messages2_1[16] = {0x02, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F};
  const uint8 messages2_2[16] = {0x02, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F};
  const uint8 messages2_3[16] = {0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F};
  const uint8 messages2_4[16] = {0x02, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F};

  // Add messages to queue1
  if (putQueue(queue1, messages1_1, sizeof(messages1_1)) != QUEUE_SUCCESS)
  {
    printf("Failed to putQueue messages1_1 in queue1\n");
  }
  if (putQueue(queue1, messages1_2, sizeof(messages1_2)) != QUEUE_SUCCESS)
  {
    printf("Failed to putQueue messages1_2 in queue1\n");
  }
  if (putQueue(queue1, messages1_3, sizeof(messages1_3)) != QUEUE_SUCCESS)
  {
    printf("Failed to putQueue messages1_3 in queue1\n");
  }
  if (putQueue(queue1, messages1_4, sizeof(messages1_4)) != QUEUE_SUCCESS)
  {
    printf("Failed to putQueue messages1_4 in queue1\n");
  }

  // Add messages to queue2
  if (putQueue(queue2, messages2_1, sizeof(messages2_1)) != QUEUE_SUCCESS)
  {
    printf("Failed to putQueue messages2_1 in queue2\n");
  }
  if (putQueue(queue2, messages2_2, sizeof(messages2_2)) != QUEUE_SUCCESS)
  {
    printf("Failed to putQueue messages2_2 in queue2\n");
  }
  if (putQueue(queue2, messages2_3, sizeof(messages2_3)) != QUEUE_SUCCESS)
  {
    printf("Failed to putQueue messages2_3 in queue2\n");
  }
  if (putQueue(queue2, messages2_4, sizeof(messages2_4)) != QUEUE_SUCCESS)
  {
    printf("Failed to putQueue messages2_4 in queue2\n");
  }

  // Print the contents of both queues
  printf("Queue 1:\n");
  printQueue(queue1);

  printf("Queue 2:\n");
  printQueue(queue2);

  // Read both queues and print remaining contents
  uint8 buffer[16];
  for (uint32 i = 0; i < 4; i++)
  {
    if (getQueue(queue1, buffer, sizeof(buffer)) == QUEUE_SUCCESS)
    {
      printf("getQueued message (queue1): ");
      for (uint32 j = 0; j < 8; j++)
      {
        printf("%02X ", buffer[j]);
      }
      printf("\n");
    }
    else
    {
      printf("Failed to getQueue\n");
    }
  }
  printQueue(queue1);

  for (uint32 i = 0; i < 4; i++)
  {
    if (getQueue(queue2, buffer, sizeof(buffer)) == QUEUE_SUCCESS)
    {
      printf("getQueued message (queue2): ");
      for (uint32 j = 0; j < 16; j++)
      {
        printf("%02X ", buffer[j]);
      }
      printf("\n");
    }
    else
    {
      printf("Failed to getQueue\n");
    }
  }
  printQueue(queue2);

  // Destroy queues
  destroyQueue(queue1);
  destroyQueue(queue2);

  return 0;
}
